#include "../include/raymo_sample/raymo_driver.h"

RaymoDriver::RaymoDriver():msg_seq_(0),start_flag_(true),state_(waitingForHead1),first_init_(false),l_motor_pos_offset_(0),r_motor_pos_offset_(0),motor_pos_initial_(false){}

RaymoDriver::~RaymoDriver()
{
    boost::mutex::scoped_lock lock(mutex_);
    recv_flag_ = false;
    if(sp_)
    {
        sp_->cancel();
        sp_->close();
        sp_.reset();
    }
    io_service_.stop();
    io_service_.reset();
}
void RaymoDriver::check_sum(uint8_t* data, size_t len, uint8_t& dest)
{
	dest = 0x00;
	for(int i=0;i<len;i++)
	{
		dest += *(data+i);
	}
}

void RaymoDriver::monitor_data(void)
{
    printf("-------------RX DATA-------------\r\n");
    printf("X Angle Speed  : %3.3f rad/s2 \r\n",((float)rx_data_.X_Angle_Speed/32768*1000)/180*3.1415926);
    printf("Y Angle Speed  : %3.3f rad/s2\r\n",((float)rx_data_.Y_Angle_Speed/32768*1000)/180*3.1415926);
    printf("Z Angle Speed  : %3.3f rad/s2\r\n",((float)rx_data_.Z_Angle_Speed/32768*1000)/180*3.1415926);
    printf("X ACC          : %3.3f m/s2\r\n",((float)rx_data_.X_Acceleration/32768*2)*9.8);
    printf("Y ACC          : %3.3f m/s2\r\n",((float)rx_data_.Y_Acceleration/32768*2)*9.8);
    printf("Z ACC          : %3.3f m/s2\r\n",((float)rx_data_.Z_Acceleration/32768*2)*9.8);
    printf("Odom           : X: %3.3fm  Y: %3.3fm  Z: %3.3frad\r\n",(float)rx_data_.Odom[0]/1000,(float)rx_data_.Odom[1]/1000,(float)rx_data_.Odom[2]/1000);
    printf("Robot Speed    : Linear: %3.3f m/s   Angular: %3.3f rad/s \r\n", (float)rx_data_.Speed[0]/1000, (float)rx_data_.Speed[1]/1000);
    printf("Motor Speed    : L : %3.3frpm  R: %3.3frpm\r\n", (float)(rx_data_.L_Motor_Speed)/1000,  (float)(rx_data_.R_Motor_Speed)/1000);
    printf("Motor Position : L : %5d    R : %5d\r\n",rx_data_.L_Motor_Position,rx_data_.R_Motor_Position);
    printf("Motor Temp     : L : %3dC    R : %3dC\r\n",rx_data_.L_Motor_Temperature, rx_data_.R_Motor_Temperature);
    printf("Motor Current  : L : %3.3fA   R : %3.3fA\r\n", (float)rx_data_.L_Motor_Current*15/1.414/2048,(float)rx_data_.R_Motor_Current*15/1.414/2048);
    printf("Motor ErrCode  : L : %4X    R : %4X\r\n", rx_data_.L_Motor_Error_Code,rx_data_.R_Motor_Error_Code);
    printf("Charge Current : %3.3fA \r\n",(float)rx_data_.Charge_Current/1000);
    printf("Bat_Current    : %3.3fA \r\n",(float)rx_data_.Battery_Current/1000);
    printf("Bat_Voltage    : %3.3fV \r\n",(float)rx_data_.Battery_Voltage/1000);
    printf("Left Motor PI  :  P[%d]   I[%d] \r\n", rx_data_.MOTOR_L_P,rx_data_.MOTOR_L_I);
    printf("Right Motor PI :  P[%d]   I[%d] \r\n", rx_data_.MOTOR_R_P,rx_data_.MOTOR_R_I);
    printf("\r\n");
    printf("Robot Status   : %4X\r\n",rx_data_.Robot_Status);
    printf("\r\n");
    printf("Drop Sense   :     %d     %d   \r\n", rx_data_.Drop_Sensor[0], rx_data_.Drop_Sensor[1]);
    printf("\r\n");
    printf("                  %d       %d   \r\n", rx_data_.Drop_Sensor[5], rx_data_.Drop_Sensor[2]);
    printf("\r\n");
    printf("                   %d     %d   \r\n", rx_data_.Drop_Sensor[4], rx_data_.Drop_Sensor[3]);
    
}
void RaymoDriver::distribute_data(uint8_t* buffer_data)
{
	memcpy(&rx_data_,buffer_data,sizeof(rx_data_));

    monitor_data();

    now_ = ros::Time::now();

    std_msgs::Int8MultiArray drop_pub_data_;

    for(int i = 0 ; i < 6; i++)
        drop_pub_data_.data.push_back(rx_data_.Drop_Sensor[i]);

    drop_sensor_pub_.publish(drop_pub_data_);

    //motor_l_speed_pub_data_.data = (float)(rx_data_.L_Motor_Speed)/1000;
    //motor_r_speed_pub_data_.data = (float)(rx_data_.R_Motor_Speed)/1000;

    //motor_l_speed_pub_.publish(motor_l_speed_pub_data_);
    //motor_r_speed_pub_.publish(motor_r_speed_pub_data_);

    imu_pub_data_.header.stamp = now_;
    imu_pub_data_.header.frame_id = imu_frame_;
    imu_pub_data_.angular_velocity.x = ((double)rx_data_.X_Angle_Speed/32768*1000/180*M_PI);
    imu_pub_data_.angular_velocity.y = ((double)rx_data_.Y_Angle_Speed/32768*1000/180*M_PI);
    imu_pub_data_.angular_velocity.z = ((double)rx_data_.Z_Angle_Speed/32768*1000/180*M_PI);

    imu_pub_data_.linear_acceleration.x = (double)(rx_data_.X_Acceleration)/32768*2*9.8;
    imu_pub_data_.linear_acceleration.y = (double)(rx_data_.Y_Acceleration)/32768*2*9.8;
    imu_pub_data_.linear_acceleration.z = (double)(rx_data_.Z_Acceleration)/32768*2*9.8;

    imu_pub_data_.orientation.x = 0;
    imu_pub_data_.orientation.y = 0;
    imu_pub_data_.orientation.z = 0;
    imu_pub_data_.orientation.w = 0;
    imu_pub_data_.orientation_covariance = {1e6, 0, 0,
                                            0, 1e6, 0,
                                            0, 0, 0.05};
    imu_pub_data_.angular_velocity_covariance = {1e6, 0, 0,
                                                 0, 1e6, 0,
                                                 0, 0, 1e6};
    imu_pub_data_.linear_acceleration_covariance = {1e-2, 0, 0,
                                                     0, 0, 0,
                                                     0, 0, 0};
    imu_pub_.publish(imu_pub_data_);

    charge_current_data_.data = ((float)rx_data_.Charge_Current)/1000;

    charge_current_pub_.publish(charge_current_data_);


    battery_state_pub_data_.header.stamp = now_;
    battery_state_pub_data_.voltage = ((float)rx_data_.Battery_Voltage)/1000;
    battery_state_pub_data_.current = ((float)rx_data_.Battery_Current)/1000;
    battery_state_pub_data_.percentage = (battery_state_pub_data_.voltage-20.0)/(29.2-20);
    //battery_state_pub_data_.temperature = ((float)rx_data_.Pms_Temp)/125; //TODO
    battery_state_pub_.publish(battery_state_pub_data_);
    
    //robot_status_pub_.publish(control_mode_pub_data_);

    transformStamped_.header.frame_id = odom_frame_;
    transformStamped_.child_frame_id  = base_frame_;
    transformStamped_.header.stamp = now_;
    transformStamped_.transform.translation.x = (float)rx_data_.Odom[0]/1000;
    transformStamped_.transform.translation.y = (float)rx_data_.Odom[1]/1000;
    transformStamped_.transform.translation.z = 0;
    tf2::Quaternion q;
    q.setRPY(0,0,(float)rx_data_.Odom[2]/1000);

    transformStamped_.transform.rotation.x = q.getX();
    transformStamped_.transform.rotation.y = q.getY();
    transformStamped_.transform.rotation.z = q.getZ();
    transformStamped_.transform.rotation.w = q.getW();

    if(publish_odom_transform_)
        br_.sendTransform(transformStamped_);

    odom_pub_data_.header.frame_id = odom_frame_;
    odom_pub_data_.child_frame_id  = base_frame_;
    odom_pub_data_.header.stamp = now_;
    odom_pub_data_.pose.pose.position.x = (float)rx_data_.Odom[0]/1000;
    odom_pub_data_.pose.pose.position.y = (float)rx_data_.Odom[1]/1000;
    odom_pub_data_.pose.pose.position.z = 0;
    odom_pub_data_.pose.pose.orientation.x = q.getX();
    odom_pub_data_.pose.pose.orientation.y = q.getY();
    odom_pub_data_.pose.pose.orientation.z = q.getZ();
    odom_pub_data_.pose.pose.orientation.w = q.getW();
    odom_pub_data_.twist.twist.linear.x = (float)rx_data_.Speed[0]/1000;
    odom_pub_data_.twist.twist.angular.z = (float)rx_data_.Speed[1]/1000;
    odom_pub_data_.twist.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 0.1 };
    odom_pub_data_.pose.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 1e3 };
    odom_pub_.publish(odom_pub_data_);

}

bool RaymoDriver::initRobot()
{
    if(sp_)
    {
        ROS_ERROR("The SerialPort is already opened!");
        return false;
    }
    sp_ = serialp_ptr(new boost::asio::serial_port(io_service_));
    sp_->open(port_name_, ec_);
    if(ec_)
     {
        ROS_ERROR_STREAM("error : port_->open() failed...port_name=" << port_name_ << ", e=" << ec_.message().c_str());
        return false;
     }
    sp_->set_option(boost::asio::serial_port_base::baud_rate(baud_rate_));
    sp_->set_option(boost::asio::serial_port_base::character_size(8));
    sp_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
    sp_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
    sp_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
    
    return true;
}

void RaymoDriver::send_speed_callback(const ros::TimerEvent&)
{
    if((ros::Time::now() - last_twist_time_).toSec() <= 1.0)
    {
            cmd_vel_mutex_.lock();
            SetMotorSpeed((int16_t)(current_twist_.linear.x*1000),(int16_t)(current_twist_.angular.z*1000)); //TODO
            cmd_vel_mutex_.unlock();
    }
    else
    {
            SetMotorSpeed(0,0);
    }

    if((ros::Time::now() - now_).toSec() >=1)
    {
        ROS_WARN_THROTTLE(1,"Didn't received odom data,Please check your connection!");
    }
}
void RaymoDriver::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_twist_ = *msg.get();
        cmd_vel_mutex_.unlock();
    }
    catch(...)
    {
        cmd_vel_mutex_.unlock();
    }
}
void RaymoDriver::estop_callback(const std_msgs::Bool::ConstPtr& msg)
{
    SetEStop(msg->data);
}
void RaymoDriver::recv_msg()
{
    uint8_t payload_size, check_num, buffer_data[255],payload_type;
    state_ = waitingForHead1;
    recv_flag_ = true;
    while(recv_flag_)
    {
	switch (state_)
        {
            case waitingForHead1:
            
                check_num = 0x00;
                boost::asio::read(*sp_.get(), boost::asio::buffer(&buffer_data[0], 1), ec_);
                state_ = buffer_data[0] == SERIAL_HEAD_A ? waitingForHead2 : waitingForHead1;
                break;
            case waitingForHead2:
          
                boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[1],1),ec_);
                
                state_ = buffer_data[1] == SERIAL_HEAD_B ? waitingForPayload : waitingForHead1;
                if(state_ == waitingForHead1)
                {
                    ROS_DEBUG("recv head2 error : %02X \r\n",buffer_data[1]);
                }
                break;
            case waitingForPayload:
            
                //boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[2],(SIZE_OF_RX_PROTOCOL-3)),ec_);
                boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[2],78),ec_);
                state_ = waitingForCheckSum;
                break;
            case waitingForCheckSum:
                
                //boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[SIZE_OF_RX_PROTOCOL-1],1),ec_);
                //check_sum(buffer_data,(SIZE_OF_RX_PROTOCOL-1),check_num);
                //state_ = buffer_data[SIZE_OF_RX_PROTOCOL-1] == check_num ? handlePayload : waitingForHead1;
                boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[80],1),ec_);
                check_sum(buffer_data,80,check_num);
                state_ = handlePayload;
                //state_ = buffer_data[80] == check_num ? handlePayload : waitingForHead1;
                if(state_ == waitingForHead1)
                {
                    ROS_WARN("check sum error : %02X  cala is %02x\r\n",buffer_data[80],check_num);
                }
                break;
            case handlePayload:
                distribute_data(buffer_data);
                state_ = waitingForHead1;
                break;
            default:
                state_ = waitingForHead1;
                break;
        }

    }
}

void RaymoDriver::loop()
{
    uint8_t stop_buf[20];
    ros::NodeHandle nh_;
    ros::NodeHandle nh_p_("~");
    nh_p_.param<std::string>("port_name",port_name_,std::string("/dev/ttyUSB0"));
    nh_p_.param<std::string>("odom_frame",odom_frame_,std::string("odom"));
    nh_p_.param<std::string>("base_frame",base_frame_,std::string("base_link"));
    nh_p_.param<std::string>("imu_frame",imu_frame_,std::string("base_imu_link"));
    
    nh_p_.param<int>("baud_rate",baud_rate_,115200);
    nh_p_.param<int>("control_rate",control_rate_,50);
    nh_p_.param<int>("acc",default_acc_,2);
    nh_p_.param<int>("dacc",default_dacc_,3);

    nh_p_.param<bool>("publish_odom_transform",publish_odom_transform_,true);

    if(initRobot())
    {
        odom_pub_    = nh_.advertise<nav_msgs::Odometry>("odom",10);
        //battery_pub_ = nh_.advertise<std_msgs::Float32>("voltage",1);
        imu_pub_     = nh_.advertise<sensor_msgs::Imu>("/raymo/imu",10);
        charge_current_pub_ = nh_.advertise<std_msgs::Float32>("/raymo/charge_current",10);
        battery_state_pub_ = nh_.advertise<sensor_msgs::BatteryState>("/raymo/battery_state",10);

        //motor_l_speed_pub_ = nh_.advertise<std_msgs::Float32>("/raymo/motor_l_speed",10);
        //motor_r_speed_pub_ = nh_.advertise<std_msgs::Float32>("/raymo/motor_r_speed",10);

        drop_sensor_pub_   = nh_.advertise<std_msgs::Int8MultiArray>("/raymo/drop_sensor",10);

        cmd_sub_     = nh_.subscribe<geometry_msgs::Twist>("cmd_vel",1,&RaymoDriver::cmd_vel_callback,this);
        //estop_sub_   = nh_.subscribe<std_msgs::Bool>("/raymo/estop",10,&RaymoDriver::estop_callback,this);



        ros::Duration(0.02).sleep();
	
        ros::Timer send_speed_timer = nh_.createTimer(ros::Duration(1.0/control_rate_),&RaymoDriver::send_speed_callback,this);

        boost::thread recv_thread(boost::bind(&RaymoDriver::recv_msg,this));
        ROS_INFO("Robot Running!");
        ros::spin();
	    SetMotorSpeed(0,0);
        return ;
    }
}
bool RaymoDriver::SetMotorSpeed(int16_t linear_speed, int16_t angular_speed)
{

    uint8_t send_buff[8];
    send_buff[0] = 0x55;
    send_buff[1] = 0xFF;
    send_buff[2] = 0x71;
    send_buff[3] = linear_speed&0xff;
    send_buff[4] = (linear_speed>>8)&0xff;
    send_buff[5] = angular_speed&0xff;
    send_buff[6] = (angular_speed>>8)&0xff;
    check_sum(send_buff,7,send_buff[7]);

    boost::asio::write(*sp_.get(),boost::asio::buffer(send_buff,8),ec_);
    ros::Duration(0.02).sleep();
    if(ec_)
    {
        ROS_DEBUG("Send Speed buffer Error! what: [%s] \r\n",ec_.message().c_str());
        ec_.clear();
        return false;
    }

    return true;

}
bool RaymoDriver::SetEStop(bool estop)
{
    uint8_t send_buff[8];
    send_buff[0] = 0xFF;
    send_buff[1] = 0x55;
    send_buff[2] = 0x81;
    send_buff[3] = 0x00;
    send_buff[4] = 0x00;
    send_buff[5] = 0x00;
    send_buff[6] = estop ? 0x01 : 0x00;
    check_sum(send_buff,7,send_buff[7]);
    boost::asio::write(*sp_.get(),boost::asio::buffer(send_buff,8),ec_);
    ros::Duration(0.02).sleep();
    if(ec_)
    {
        ROS_DEBUG("Send EStop buffer Error! what: [%s] \r\n",ec_.message().c_str());
        ec_.clear();
        return false;
    }
    return true;
}
int main(int argc,char ** argv)
{
     ros::init(argc, argv, "raymo_demo_node");
     RaymoDriver driver;
     driver.loop();
     return 0;
}
